#include "stdafx.h"
#include "Element.h"
#include "SimpleMap.h"
#include "PathFinder.h"
Element::Element(Core::Position start,Evironment* e)
	:CommandReceiver(e),now(start)
{
	//currentAction=stop;
	ParamList* p=new ParamList();
	p->Add("speed",Param(Core::DOUBLE,new double(5)));
	this->AddAction(new Move(this,p));
	this->AddDefaultAction(new Stop());
	//this->AddCommand(new Stop());
}

Move::Move(Element* e,ParamList* list)
:Action(e,list),x(e->GetPosition().x),y(e->GetPosition().y)
{
	try
	{
		ParamList& p=*list;
		speed=(p["speed"].GetDouble());
	}
	catch(std::exception e)
	{
		exit(0);
	}
}
void Move::PreExecute(Evironment* e,ParamList* list)
{
	if(!path.empty()) 
	{
		path.clear();
	}
	try
	{
		ParamList& p=*list;
		mDes=(p["des"].GetPosition());
		//des=e->GetGridPosition(des);
	}
	catch(std::exception e)
	{
		exit(0);
	}
	Core::PathFinder<Evironment::LogicPart> finder((e->GetLogicMap()));
	finder.GetPach(e->BlockPosition(receiver->GetPosition()),e->BlockPosition(mDes),path);
	if(path.size()>0)
	{
		pPos=path.begin();
		pPos++;
		mStep=e->GetGridPositionFromBlockPosition(pPos->pos);
		pPos++;
	}
	else
	{
		receiver->SetToDefault();
	}
}
void Move::Execute(Evironment* e,ParamList* list)
{
	Core::Position& des=mStep;
	Core::Position now=receiver->GetPosition();
	if(abs(now.x-des.x)<speed&&abs(now.y-des.y)<speed)
	{
		if(pPos==path.end())
		{
			receiver->SetToDefault();
			now.x=des.x;
			now.y=des.y;
			receiver->SetPosition(now);
			return;
		}
		else
		{
			des=e->GetGridPositionFromBlockPosition(pPos->pos);
			pPos++;
		}
	}
	
	{
		double dy,dx;
		dy=des.y-now.y;
		dx=des.x-now.x;
		double d=sqrt(dx*dx+dy*dy);
		if(d<1e-10) return;
		dy=dy/d*speed;
		dx=dx/d*speed;
		x+=dx;
		y+=dy;
		now.x=(int)x;
		now.y=(int)y;

		/*now.x=des.x;
		now.y=des.y;*/
	}
	receiver->SetPosition(now);

}

std::string Move::GetId()
{
	return "move";
}